%This program demonstrates the performances of LLS,KF and EKF
%in chapter 4
%Revised 10-30-2006
%Copyright Ramond Wong, Wu Shiqian
clear all
close all
rand('seed',5);x1=rand(1,200)*20-10;
x=[-10:0.1:-0.1 0.1:0.1:10];y=x;p=[x;y];
%function to be approximated
t=sin(x)./x.*sin(y)./y;
%Set initial parameter values
kdmax=14;kdmin=0.5;gama=0.95;beta=0.9;emax=1;emin=0.02;k=1.1;
kw=1.1;kerr=0.0015;eemax=0;ekfmax=0;aemax=0;
[r,q]=size(p);[s2,q]=size(t);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Extended Kalman Filter Algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t0=clock;
ALLIN=[];ALLOUT=[];
%For the first rule
CRBF=p(:,1)';alter=1;width=[];width(1)=2;
aa=100;cc=0.01;
WEIGHT=rand((r+1),s2);
covP=aa*eye(r+1);covPW=cc*eye(2);
for i=1:q
    data=i;IN=p(:,i);OUT=t(:,i);
    ALLIN=[ALLIN IN];
    ALLOUT=[ALLOUT OUT];
    [r,N]=size(ALLIN);  [u,r]=size(CRBF);
    RBFOUT=RBF(dist(CRBF,ALLIN),1./width');
    pa=RBFOUT./(ones(u,1)*sum(RBFOUT));
    pTSK=transf(pa,ALLIN);
    if alter==1
        WEIGHT=rand(u*(r+1),s2);
        covP=aa*eye(u*(r+1));
        covPW=cc*eye(u);
        for iter=1:i
            Kgain=(covP*pTSK(:,iter))/(1+pTSK(:,iter)'*covP*pTSK(:,iter));
            WEIGHT=WEIGHT+Kgain*(ALLOUT(:,iter)'-pTSK(:,iter)'*WEIGHT);
            covP=covP-Kgain*pTSK(:,iter)'*covP;
            HWall=[];
            KWgain=[];
            for o=1:s2
                fu=[1 ALLIN(:,iter)']*(reshape(WEIGHT(:,o),u,(r+1)))';
                F=[];
                for ii=1:u
                    for jj=1:u
                        F(ii,jj)=fu(:,ii)-fu(:,jj);
                    end
                end
                HW=(-2*(dist(CRBF,ALLIN(:,iter))).^2).*(1./width').*pa(:,iter).*(F*pa(:,iter));
                HWall=[HWall HW];
                KWgain=[KWgain (covPW*HW)/(1+HW'*covPW*HW)];
            end
            width=1./(1./width'+KWgain*(ALLOUT(:,iter)-WEIGHT'*pTSK(:,iter)))';
            covPW=covPW-KWgain*HWall'*covPW;
        end
        alter=0;
    else
        Kgain=(covP*pTSK(:,i))/(1+pTSK(:,i)'*covP*pTSK(:,i));
        WEIGHT=WEIGHT+Kgain*(ALLOUT(:,i)'-pTSK(:,i)'*WEIGHT);
        covP=covP-Kgain*pTSK(:,i)'*covP;
        HWall=[];
        KWgain=[];
        for o=1:s2
            fu=[1 ALLIN(:,i)']*(reshape(WEIGHT(:,o),u,(r+1)))';
            F=[];
            for iii=1:u
                for jjj=1:u
                    F(iii,jjj)=fu(:,iii)-fu(:,jjj);
                end
            end
            HW=(-2*(dist(CRBF,ALLIN(:,i))).^2).*(1./width').*pa(:,i).*(F*pa(:,i));
            HWall=[HWall HW];
            KWgain=[KWgain (covPW*HW)/(1+HW'*covPW*HW)];
        end
        width=1./(1./width'+KWgain*(ALLOUT(:,i)-WEIGHT'*pTSK(:,i)))';
        covPW=covPW-KWgain*HWall'*covPW;
    end
    ALLEKFOUT=WEIGHT'*pTSK;
    rmsekf(i)=sqrt(sumsqr(ALLOUT-ALLEKFOUT)/(i*s2));
    nekf(i)=u;
    %for computation of per data set error
    RBFOUT1=RBF(dist(CRBF,IN),1./width');
    pa1=RBFOUT1/sum(RBFOUT1);
    pTSK1=transf(pa1,IN);
    ACTOUT=WEIGHT'*pTSK1;
    ekf(i)=sqrt(sumsqr(OUT-ACTOUT)/s2);
    z3=ekf(i)-ekfmax;
    if z3>=0
        ekfmax=ekf(i);
    end
    [d,ind]=min(dist(CRBF,IN));
    kd=max(kdmax*gama^(i-1),kdmin);
    if d>kd
        ke=max(emax*beta^(i-1),emin);
        if ekf(i)>ke %generate new neuron
            CRBF=[CRBF;IN'];
            width=[width k*d];
            [u,c]=size(CRBF);
            if u*(r+1)<=N
                RBFOUT2=RBF(dist(CRBF,ALLIN),1./width');
                pa2=RBFOUT2./(ones(u,1)*sum(RBFOUT2));
                pTSK2=transf(pa2,ALLIN);
                [W,A]=orthogonalize(pTSK2');
                SSW=sum(W.*W);
                SStT=sum(ALLOUT'.*ALLOUT')';
                err=(((W'*ALLOUT')'.^2)./(SStT*SSW))';
                err1=zeros(u,s2*(r+1));err1(:)=err;
                err2=sqrt(sum(err1'.*err1')/(s2*(r+1)));
                No=find(err2<kerr);
                if ~isempty(No)
                    CRBF(No,:)=[];  width(:,No)=[]; err1(No,:)=[];
                    [u,v]=size(CRBF);   %update number of neurons
                end
            end
            alter=1;
        end
    end
end
eklmtime=etime(clock,t0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LLS Algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t0=clock;
ALLIN=[];ALLOUT=[];CRBF=[];width=[];width(1)=2;
ALLIN=p(:,1);ALLOUT=t(:,1);
CRBF=p(:,1);w1=CRBF';neutr(1)=1;
a0=RBF(dist(w1,ALLIN),1./width');
a01=[a0 p(:,1)'];
w2=ALLOUT/a01';
a02=w2*a01';
sse(1)=sumsqr(ALLOUT-a02)/s2;rmse(1)=sqrt(sse(1));
for i=2:q
    data=i;
    IN=p(:,i);OUT=t(:,i);
    ALLIN=[ALLIN IN];ALLOUT=[ALLOUT OUT];
    [r,N]=size(ALLIN);  [s,r]=size(w1);
    dd=dist(w1,IN);
    [d,ind]=min(dd);
    kd=max(kdmax*gama.*(i-1),kdmin);
    ai=RBF(dist(w1,IN),1./width');
    aai=sum(ai);    aai1=ai/aai;
    ai1=transf(aai1,IN);
    ai2=w2*ai1;
    errout=t(:,i)-ai2;  errout1=errout.*errout; errout2=sum(errout1)/s2;
    e(i)=sqrt(errout2);
    z2=e(i)-eemax;
    if z2>=0
        eemax=e(i);
    end
    ke=max(emax*beta.^(i-1),emin);
    if d>kd
        if e(i)>ke
            CRBF=[CRBF IN];
            wb=k*d;
            width=[width wb];
            w1=CRBF';
            [u,v]=size(w1);
            A=RBF(dist(w1,ALLIN),1./width');
            A0=sum(A);
            A1=A./(ones(u,1)*A0);
            A2=transf(A1,ALLIN);
            if u*(r+1)<=N
                tT=ALLOUT';
                PAT=A2';
                [W,AW]=orthogonalize(PAT);
                SSW=sum(W.*W)';
                SStT=sum(tT.*tT)';
                err=((W'*tT)'.^2)./(SStT'*SSW');
                errT=err';  err1=zeros(u,s2*(r+1));
                err(:)=err(T);
                err21=err1'; err22=sum(err21.*err21)/(s2*(r+1));
                err23=sqrt(err22);
                No=find(err23<kerr);
                if ~isempty(No)
                    CRBF(:,No)=[];w1(No,:)=[];width(:,No)=[];
                    err21(:,No)=[];
                    [uu,vv]=size(w1);
                    AA=RBF(dist(w1,ALLIN),1./width');
                    AA0=sum(AA);AA1=AA./(ones(uu,1)*AA0);
                    AA2=transf(AA1,ALLIN);
                    w2=ALLOUT/AA2;
                    outAA2=w2*AA2;
                    sse0=sumsqr(ALLOUT-outAA2)/(i*s2);
                    rmse0=sqrt(sse0);
                    tr(i)=rmse0;neutr(i)=uu;
                    w2T=w2';ww2=zeros(uu,s2*(r+1));
                    ww2(:)=w2T; w21=ww2';
                else
                    w2=ALLOUT/A2;outA2=w2*A2;
                    sse0=sumsqr(ALLOUT-outA2)/(i*s2);
                    rmse0=sqrt(sse0);tr(i)=rmse0;
                    neutr(i)=u; w2T=w2';
                    ww2=zeros(u,s2*(r+1));
                    ww2(:)=w2T; w21=ww2';
                end
            else
                w2=ALLOUT/A2; outA2=w2*A2;
                sse0=sumsqr(ALLOUT-outA2)/(i*s2);
                rmse0=sqrt(sse0);tr(i)=rmse0;
                neutr(i)=u; w2T=w2'; ww2=zeros(u,s2*(r+1));
                ww2(:)=w2T; w21=ww2';
            end
        else
            a=RBF(dist(w1,ALLIN),1./width');
            a0=sum(a);a1=a./(ones(s,1)*a0);
            a2=transf(a1,ALLIN);
            w2=ALLOUT/a2;outa2=w2*a2;
            sse1=sumsqr(ALLOUT-outa2)/(i*s2);
            rmse1=sqrt(sse1);tr(i)=rmse1;
            neutr(i)=s;
        end
    else
        if e(i)>ke
            width(ind)=kw*width(ind);
            aa=RBF(dist(w1,ALLIN),1./width');
            aa0=sum(aa);aa1=aa./(ones(s,1)*aa0);
            aa2=transf(aa1,ALLIN);
            w2=ALLOUT/aa2; outaa2=w2*aa2;
            sse2=sumsqr(ALLOUT-outaa2)/(i*s2);
            rmse2=sqrt(sse2);tr(i)=rmse2;
            neutr(i)=s;
        else
            aa1=RBF(dist(w1,ALLIN),1./width');
            aa01=sum(aa1); aa11=aa1./(ones(s,1)*aa01);
            aa21=transf(aa11,ALLIN);
            w2=ALLOUT/aa21; outaa21=w2*aa21;
            sse3=sumsqr(ALLOUT-outaa21)/(i*s2);
            rmse3=sqrt(sse3);tr(i)=rmse3;
            neutr(i)=s;
        end
    end
end
LSEtime=etime(clock,t0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LLS-Kalman Filter Algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t0=clock; ALLIN=[]; ALLOUT=[];
%For the first rule
CRBF=p(:,1)';alter=1;width=[];width(1)=2;
aa=200; bb=0.001;
for i=1:q
    data=i; ds(i)=i;
    IN=p(:,i); OUT=t(:,i);
    ALLIN=[ALLIN IN];ALLOUT=[ALLOUT OUT];
    [r,N]=size(ALLIN); [u,r]=size(CRBF);
    RBFOUT=RBF(dist(CRBF,ALLIN),1./width');
    pa=RBFOUT./(ones(u,1)*sum(RBFOUT));
    pTSK=transf(pa,ALLIN);
    if alter==1
        WEIGHT=bb*ones(u*(r+1),1);
        Si=aa*eye(u*(r+1));
        for iter=1:i
            Si=Si-(Si*pTSK(:,iter)*pTSK(:,iter)'*Si)/(1+pTSK(:,iter)'*Si*pTSK(:,iter));
            WEIGHT=WEIGHT+Si*pTSK(:,iter)*(ALLOUT(:,iter)-pTSK(:,iter)'*WEIGHT);
        end
        alter=0;
    else
        Si=Si-(Si*pTSK(:,i)*pTSK(:,i)'*Si)/(1+pTSK(:,i)'*Si*pTSK(:,i));
        WEIGHT=WEIGHT+Si*pTSK(:,i)*(ALLOUT(:,i)-pTSK(:,i)'*WEIGHT);
    end
    ALLESTOUT=WEIGHT'*pTSK;
    rmse(i)=sqrt(sumsqr(ALLOUT-ALLESTOUT)/(i*s2));
    neuron(i)=u;
    %for computation of per data set error
    RBFOUT1=RBF(dist(CRBF,IN),1./width');
    pa1=RBFOUT1/sum(RBFOUT1);
    pTSK1=transf(pa1,IN); %convert to TSK model
    ACTOUT=WEIGHT'*pTSK1;
    ae(i)=sqrt(sumsqr(OUT-ACTOUT)/s2);
    z1=ae(i)-aemax;
    if z1>=0
        aemax=ae(i);
    end
    [d,ind]=min(dist(CRBF,IN));
    kd=max(kdmax*gama^(i-1),kdmin);
    if d>kd
        ke=max(emax*beta^(i-1),emin);
        if ae(i)>ke  %generate new neuron
            CRBF=[CRBF;IN'];
            width=[width k*d];
            [u,v]=size(CRBF);
            if u*(r+1)<=N
                RBFOUT2=RBF(dist(CRBF,ALLIN),1./width');
                pa2=RBFOUT2./(ones(u,1)*sum(RBFOUT2));
                pTSK2=transf(pa2,ALLIN);
                [W,A]=orthogonalize(pTSK2');
                SSW=sum(W.*W);
                SStT=sum(ALLOUT'.*ALLOUT')';
                err=(((W'*ALLOUT')'.^2)./(SStT*SSW))';
                err1=zeros(u,s2*(r+1));err1(:)=err;
                err2=sqrt(sum(err1'.*err1')/(s2*(r+1)));
                No=find(err2<kerr);
                if ~isempty(No)
                    CRBF(No,:)=[];width(:,No)=[];
                    err1(No,:)=[];[u,v]=size(CRBF);
                end
            end
            alter=1;
        end
    elseif ae(i)>ke
        width(ind)=kw*width(ind);
        alter=1;
    end
end
lseklmtime=etime(clock,t0);
meanerr=mean(e);meanae=mean(ae);meanekf=mean(ekf);
figure,plot(ds,neutr,'r',ds,neuron,'ro',ds,nekf,'r+');
title('Fuzzy rule generation');
xlabel('Sample patterns');
figure,plot(ds,e,'r',ds,ae,'ro',ds,ekf,'r+');
title('Actual Output Error');
xlabel('Sample patterns');
% figure,plot(ds,tr,'r',ds,rmse,'ro',ds,rmsekf,'r+');
figure,plot(ds,tr,'r');hold on
plot(ds,rmse,'ro');hold on
plot(ds,rmsekf,'r+');
title('Root mean squared error');
xlabel('Sample patterns');